When work such as assembly is performed using a manipulator having a hand, it is necessary to adjust or control the force or stiffness of the hand in some manner. In other words, when only position control is performed, a conventional industrial manipulator simply has high position rigidity and operates by disregarding the affects of the workpiece to be handled. The result is that, if the manipulator is required to perform assembly work such as insertion, the manipulator movement cannot adapt to the shape of the workpiece. In addition, if there is a resulting large deviation in the position, the workpiece may be damaged. Normally, a spring or similar resilient object is mounted on the manipulator hand as a buffer in case of contact in order to adjust the stiffness of the hand. Compliance control is a method used to perform the stiffness adjustment of the hand for matching the hand stiffness with the external environment, while obviating the need for a mechanism such as the spring, but at the same time controlling the manipulator as if the hand contained a spring.
The balance of force in a dynamic system involves not only force from the stiffness of a spring, but also those forces emanating from sources such as a damper, inertia, and a force source. It is necessary to take all these parameters into consideration when matching the hand stiffness with the external environment. The compliance for matching with the external environment is represented by a second-order equation of motion as shown schematically in FIG. 2 and equation 1 below. Generally, translational and rotational motion are processed separately for each coordinate axis of the Cartesian coordinate system for which compliance control is sought. That is, EQU M(p-p.sub.r)+K.sub.d (p-p.sub.r) +K.sub.s (p-p.sub.r)=K.sub.f (f.sub.r -f). (Eq. 1) EQU C.sup.c M C.sup.T (p-p.sub.r)+C.sup.c K.sub.d C.sup.T (p-p.sub.r) +C.sup.c K.sub.s C.sup.T (p-p.sub.r) =C.sup.c K.sub.f C.sup.T (f.sub.r -f).
where,
P.sub.r : Goal value of hand position (6 vectors) PA0 p : Hand position (6 vectors) PA0 f.sub.r : Goal value of force applied on hand (6 vectors) PA0 f : Force applied on hand PA0 M : Imaginary mass.inertia in the global coordinate system (6.times.6 matrix) PA0 K.sub.d : Damping coefficient in the global coordinate system (6.times.6 matrix) PA0 K.sub.s : Spring coefficient in the global coordinate system (6.times.6 matrix) PA0 K.sub.f : Compliance selection matrix in the global coordinate system (6.times.6 matrix) PA0 C : Compliance coordinate system PA0 c.sub.M : Imaginary mass.inertia in the compliance coordinate system (6.times.6 diagonal matrix) PA0 c.sub.K.sbsb.d : Damping coefficient in the compliance coordinate system (6.times.6 diagonal matrix) PA0 c.sub.K.sbsb.s : Spring coefficient in the compliance coordinate system (6.times.6 diagonal matrix) PA0 c.sub.K.sbsb.f : Compliance selection matrix in the compliance coordinate system (6.times.6 diagonal matrix of which diagonal element is 1 or 0)
In the present case, the compliance selection matrix (c.sub.K.sbsb.f) is analogous to a switch for selecting whether the force should be considered (1) or not considered (0) in the compliance coordinate. If compliance is considered for each axis, it is the same type of suspension as that found in cars and many other products. In other words, the purpose of the compliance control may be said, as shown in FIG. 2, to control the manipulator as if the hand had a suspension mechanism consisting of a spring, a damper, an imaginary mass and a force source, and furthermore to control the manipulator so as to satisfy the motion equation of equation (1).
Conventional compliance control simplifies the equation, and mainly considers mechanisms as either a spring or a damper.
In addition, the publication "Force Feedback Control of Robot Arm" by Kouichiro Sugimoto, Journal of the Society of Instrument and Control Engineers, Vol. 25, No. 1, Jan., 1986 describes a technique for performing compliance control by detecting external forces with a force sensor for velocity control of a manipulator.
According to the above publication, motion equation (1), in the global coordinate system, including values of the detected force, is integrated to obtain a velocity which is converted into a velocity in the joint coordinate system by the Jacobian matrix, to then perform ordinary velocity control. When equation (1) is simplified by assuming K.sub.f to be a unit matrix, and the goal values of velocity, acceleration and force (P.sub.r, P.sub.r, f.sub.r) to be 0, the equation reduces to EQU Mp=-K.sub.d p+K.sub.s (p.sub.r-p) -f (Eq. 2)
The following equation is obtained by multiplying both sides of the equation by an inverse matrix of M and then by performing integration. EQU p=M.sup.-1 .intg.[-K.sub.d p+K.sub.s (P.sub.r- p)-f]dt (Eq. 3)
The equation may then be represented in a different form as: EQU p.sub.n =TM.sup.-1 [K.sub.s (P.sub.r -p) -f]+(I-Tm.sup.-1 K.sub.d) P.sub.n-1 (Eq. 4)
wherein, T is sampling time.
When the velocity (P.sub.n) is converted to that in the joint coordinate system by the Jacobian matrix, the goal angular velocity of the joint (q.sub.n) is obtained as: ##EQU1## Namely, any compliance of a second-order mechanism is realized by velocity control of the manipulator by the joint angular velocity (q.sub.n) that is obtained by substituting fed-back position (p) and force (f) in equation (5).
However, this approach still has a problem. The control method may become unstable when subjected to friction. Considering position stability, FIG. 3 shows a block diagram of a position stability control technique. It should be noted that the position feedback signal is returned to the step where the trajectory is generated. When terms relating to the position feedback are extracted from the equation (3), the following equation is obtained. EQU P.sub.rc =M.sup.-K.sub.s .intg. (P.sub.r -p) dt (Eq. 6)
The velocity in the global coordinate system is represented as p.sub.rc to avoid confusion with the feedback value p. The goal angular velocity of the joint q.sub.rc is obtained by converting p.sub.rc for the joint coordinate system. EQU q.sub.rc =J (q).sup.-1 M.sup.-1 K .sub.s .intg. (p.sub.r -p) dt (Eq. 7)
Assuming the velocity feedback gain to be K as shown in FIG. 3, the torque to be applied to the joint is as follows: ##EQU2## Alternatively, assuming the position feedback K.sub.2 to be EQU K.sub.2 =KJ(q).sup.-1 M.sup.-1 K.sub.s (Eq. 9)
then EQU K.sub.2 .intg. (p.sub.r -p) dt (Eq. 10)
Thus, for the position feedback, integral control is provided.
Generally, an industrial manipulator has high friction because of mechanisms such as reduction gears. Suppose the static friction at the joint to be .tau..sub.fs, the dynamic friction to be .tau..sub.fd. Now, it is assumed that the position p slightly deviates from its goal value p.sub.r. In this case, the joint initially stops because of the static friction, but the right side of the equation (10) is integrated over time and increases as time elapses until becoming equal to the static friction torque T.sub.fs at a time t.sub.i. That is, EQU T.sub.fs =.tau.=K.sub.2 (p.sub.r -p) t.sub.i (Eq. 11)
Thereafter, the joint torque becomes larger than the static friction, and the joint starts to move. The friction during operation or the dynamic friction torque .tau..sub.fd is smaller than the static friction torque .tau..sub.fs, so that the joint overshoots the goal value p.sub.r, and goes out of position to the opposite side. Then, when torque in the opposite direction increases and exceeds the static friction as time passes, the joint starts to move in the opposite direction. Under this control, such operation continues indefinitely. FIG. 4 shows this operation. Such operation occurs best when the position gain K.sub.2 is small. Conversely, it is sufficient to increase the position gain K.sub.2 in order to reduce the cycle of the operation and the amplitude of the position deviation. In ordinary integral control, the position gain K.sub.2 is increased to a value not causing oscillation. In other words, it is not preferable to significantly change the position feedback gain when considering controllability.
The purpose of the compliance control is to adjustably set the spring (stiffness) and the damper (damping) of the hand during manipulator operation. According to the control technique, the stiffness of the spring is varied by changing the spring constant K.sub.s in equation (3), i.e. to vary the position gain K.sub.2 in equation (9). For example, assume that a weak spring is desired. If the spring constant K.sub.s is made smaller, this necessarily makes the position gain K.sub.2 smaller, and the above-mentioned oscillation becomes unavoidable. Referring to the equation (9), another limiting factor of the position gain K.sub.2 involves a matrix to convert values from the global coordinate system to the joint coordinate system, that is, use of the inverse Jacobian matrix J(q).sup.-1. This matrix is a function of the angle of the joint, and its value varies significantly from 0 to infinity depending upon the configuration of the manipulator. Therefore, the value of position gain K.sub.2 also varies significantly. As a result, rigidity that is set for one manipulator configuration sometimes cannot be used for another manipulator configuration. This result follows from the fact that the stiffness of the spring in the compliance control varies as the position gain K.sub.2 is changed.
Therefore, in order to overcome these shortcomings, consideration must be made when the generating a trajectory to separate the requirements of the specified compliance from the requirements of the feedback gain.